package robot;
import java.io.IOException;

import lejos.nxt.LCD;
import lejos.nxt.SensorPort;
import lejos.nxt.TouchSensor;

public class OnBoard {
	private static final int TOO_CLOSE_TO_WALL = 500999;
	private static final int BACK_UP_COMMAND = 102005;
	static Robot robot = new Robot();
	static Connection connection = new Connection();
	static SensorController sensorController = new SensorController();
	public static boolean keepItRunning = true;

	public static void main(String[] args) throws IOException, InterruptedException
	{

		//connect to bluetooth code moved to connection class 
		connection.connect();

		new CommandThread("commandListener").start();
        new SensorThread("sensorListener").start();
		/*int n; //holds the incoming command
		boolean keepItRunning = true;

		//THREAD 2
		//look for the command
		while (keepItRunning){
			n= connection.getNextCommand();
			if (n!=-1){
				LCD.drawInt(n,7,0,1);
				robot.execute(n);
				System.out.println("N : "+n);
			}
		}
		//END THREAD 2
		
		
		
		//THREAD 1
		//look for sensor data in one loop/thread
		while (keepItRunning){
			checkErrors();
		}
		//END THREAD 1
		*/
		
		connection.shutdown(); //unreachable code 

	}

	public static void checkErrors() {
		if (connection.hasWeakConnection())
		{
			System.out.println("Haz week connection!");

			robot.halt();
		}

		//sensor detects something close by
		if(sensorController.tooClose())
		{
			System.out.println("Stopped!");
			robot.halt(); //halt
			robot.execute(BACK_UP_COMMAND); //back up 5cm.
			connection.sendIntToBase(TOO_CLOSE_TO_WALL); //ultrasonic Sensor too close
		}	
		
		//if button on the back is pushed, we hit a wall
		if(robot.touch.isPressed()){
			robot.halt();
			connection.sendIntToBase(TOO_CLOSE_TO_WALL); //ultrasonic Sensor too close
		}
	}
	
	public static Connection getConnection(){
		return connection;
	}
	
	public static Robot getRobot(){
		return robot;
	}
}
